/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Robotics
 */
public class DriveAutonomousDistance extends CommandBase {
    
    private double timeLeft = 0;
    private double distanceFromWall = 0.0;
    private boolean isDone;
    
    public DriveAutonomousDistance(double StartTime, double Distance) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(chassis);
        requires(rangefinder);
        requires(gyroscope);
        
        timeLeft = 5 - (System.currentTimeMillis() - StartTime) / 1000;
        distanceFromWall = Distance;
    }

    // Called just before this Command runs the first time
    protected void initialize(){
        //distanceFromWall = SmartDashboard.getNumber("Autonomous Distance From Wall: ", 25.0);
        setTimeout(timeLeft);
        isDone = false;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        SmartDashboard.putBoolean("In Shooting Range", false);
        while(rangefinder.getRange() > distanceFromWall){
            double currGyroAngle = gyroscope.getAngle();
            if(currGyroAngle != Math.abs(currGyroAngle)){
                while(currGyroAngle < -360){
                    currGyroAngle += 360;
                }
            }else{
                while(currGyroAngle > 360){
                    currGyroAngle -= 360;
                }
            }
            currGyroAngle /= 360;
            chassis.straight(currGyroAngle);      
        }
        isDone = true;
        /*SmartDashboard.putBoolean("In Range", false);
        while(rangefinder.getRange() > distanceFromWall){
            SmartDashboard.putNumber("Distance Left: ", rangefinder.getRange());
            /*if(gyroscope.getAngle() > 0.5){
                chassis.turnAngled(-gyroscope.getAngle());
            }else if(gyroscope.getAngle() < -0.5){
                chassis.turnAngled(-gyroscope.getAngle());
            }
            chassis.straight(gyroscope.getAngle() / 4);
        }
        SmartDashboard.putBoolean("In Range", true);*/
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return isTimedOut() | isDone;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
